#include<ros/ros.h>

#include <tf/transform_listener.h>

#include<geometry_msgs/PoseStamped.h>

#include<hri_msgs/Torsos.h>
#include <hri_msgs/Torso.h>

#include<visualization_msgs/MarkerArray.h>

class TorsoVisualizer
{
public:
    TorsoVisualizer();
    bool init();
    int run();

private:
    visualization_msgs::Marker createMarker(geometry_msgs::Pose torso_pose,int id);


   void torsoSubscriberCB(const hri_msgs::TorsosConstPtr &torsos);
   geometry_msgs::PoseStamped transform(geometry_msgs::Pose pose);

   ros::NodeHandle nh;

   ros::Subscriber torsoSubscriber;
   ros::Publisher torso_marker_publisher;
   tf::TransformListener *transform_listener;
};
